/*
 * calibration.c
 *
 *  Created on: 2017年9月13日
 *      Author: shine
 */
#include "calibration.h"
#include "Configuration.h"
#include "microprocessor_definitions.h"
#include <stdio.h>
#include <sys/stat.h>
#include "microprocessor_commands.h"
#include "qxdatastruct.h"
#include "errorCodes.h"
#include<time.h>
#include<sys/types.h>

// 大致一样
uint16_t calculateChecksumForPartA(int joint_id)
{
	uint16_t ret = 0;
	uint8_t* p1 = (uint8_t*)&calval[joint_id].flash_write_count;
	uint8_t* p2 = (uint8_t*)&calval[joint_id].checksum_part_A;

	while (p1 < p2) {
		ret += *p1;
		p1++;
	}
	return ret;
}

// 大致一样
uint16_t calculateChecksumForPartC(int joint_id)
{
	uint16_t ret = 0;
	uint8_t* p1 = (uint8_t*)&calval[joint_id].joint_revolution_counter;
	uint8_t* p2 = (uint8_t*)&calval[joint_id].checksum_part_C;

	while (p1 < p2) {
		ret += *p1;
		p1++;
	}
	return ret;
}

// 大致一样
uint16_t calculateChecksumForPartD(int joint_id)
{
	uint16_t ret = 0;

	uint8_t* p1 = (uint8_t*)&calval[joint_id].data[0];
	uint8_t* p2 = (uint8_t*)&calval[joint_id].checksum_part_D;

	while (p1 < p2) {
		ret += *p1;
		p1++;
	}
	return ret;
}

// 完全一样
void joint_calibration_received_notification()
{
	int i;
	for (i = 0; joint_partD_calibration_received[i]; i) {
		if (++i == 6) {
			conf.calibration.calibrationDataReadyToBeWrittenToFile = 1;
			return;
		}
	}
	conf.calibration.calibrationDataReadyToBeWrittenToFile = 0;
	return;
}

// 完全一样
void composeCalibrationDataRequestMessage(int a)
{
	outbuffer[write_position++] = calibration_data_read_offset[a];
}

// 错误
void composeCalibrationDataMessage(int joint_id)
{
	int total_len_var_10;
	uint32_t send_offset;
	uint8_t *pbuff;
	uint8_t subleng;

	subleng = write_position;
	total_len_var_10 = sizeof(struct CalibrationValues);//总共一个数据结构的长度
	//已发送多少
	send_offset = calibration_data_send_offset[joint_id];

	outbuffer[write_position++] = 0;
	outbuffer[write_position++] = send_offset;
	total_len_var_10 -= send_offset;
	/*
    for(; outbuffer[subleng] <= 7 && total_len_var_10 >= 0; outbuffer[subleng]++, total_len_var_10--)
	     outbuffer[write_position++]= *pbuff++;
	 */

	if (outbuffer[subleng] <= 7) {
		if (total_len_var_10 > 0) {
			pbuff = (uint8_t *)&calval[joint_id] + send_offset;
			do {
				total_len_var_10--;
				outbuffer[subleng]++;
				outbuffer[write_position++] = *pbuff++;
			} while(outbuffer[subleng] <= 7 && total_len_var_10 > 0);
		}
	}

}

// 完全一样
void parseCalibrationRequestData(int a)
{
	sending_calibration_data_to_joint[a] = 1;
	calibration_data_send_offset[a] = inbuffer[read_position++];
}

// 完全一样
void parseStopSendingCalibrationData(int a)
{
	sending_calibration_data_to_joint[a] = 3;
}

// 完全一样
void manufactureArtificialEmptyCalibrationData(int joint_id)
{
	uint8_t* p1 = (uint8_t*)&calval[joint_id];
	uint8_t* p2 = (uint8_t*)&calval[joint_id+1];

	do {
		*p1 = 0xFF;
		p1++;
	}while (p2 - p1);

	calval[joint_id].flash_write_count = 0;
	calval[joint_id].joint_id = joint_id;
	calval[joint_id].AD_Voltage_offset_calibration = 0x5393;
	calculateChecksumForPartA(joint_id);
	calval[joint_id].last_seen_joint_position = 0;

	calval[joint_id].checksum_part_A = 0x5393;
	calval[joint_id].joint_revolution_counter = 0;

	calculateChecksumForPartC(joint_id);
	calval[joint_id].checksum_part_C = 0;

	calculateChecksumForPartD(joint_id);
	calval[joint_id].checksum_part_D = 0;
}

// 大致一样
void calibration_set_revision_zero(int joint_id)
{
	int i = 0;
	calval[joint_id].revision = 0;  // word_80019E6[edx]
	calval[joint_id].data[12] = -1;  // word_8001A28[edx]
	conf.calibration.joint_revision[joint_id] = 0;  // (conf+0B4Ch)
	double* pd = (double*)&conf.calibration.self_test_measurement_speed[0][0];
	while (i++ < 8) {
		(pd + 8*6)[joint_id] = 0;
		(pd + 8*6*2)[joint_id] = 0;
		(pd + 8*6*3)[joint_id] = 0;
		pd[joint_id] = 0;
		pd ++;
	}
}

// 完全一样
void calibration_initialize(int joint_id)
{
	sending_calibration_data_to_joint[joint_id]   = 0;
	calibration_data_read_offset[joint_id]        = 0;
	getting_calibration_data_from_joint[joint_id] = 1;
	calibration_data_send_offset[joint_id]        = 0;
	calibration_is_ok[joint_id]                   = 1;
}

// 完全一样
int calibration_ok(int arg)
{
	int ret = 1;
	if (conf.calibration.joint_revision[arg] <= 0)
		ret = calibration_is_ok[arg];
	return ret;
}

// 完全一样
int jointHasPartDCalibration(int arg)
{
	int temp = arg;
	temp = joint_partD_calibration_received[temp];
	return temp;
}

// 完全一样
void setJointHasPartDCalibration(int a1, int a2)
{
	joint_partD_calibration_received[a1] = a2;
}

// 完全一样
int jointCompletedSelfTestWithFW1_5(int joint_id)
{
	if (jointHasPartDCalibration(joint_id)) {
		if (!calval[joint_id].data[12]) {  // word_8001A28[edx]
			if (calval[joint_id].data[15] != -1) {  // word_8001A2E[edx]
				if (calval[joint_id].data[16] != -1) {  // word_8001A30[edx]
					if (calval[joint_id].data[17] != -1) {  // word_8001A32[edx]
						return calval[joint_id].data[18] != -1;
					}
				}
			}
		}
	}
	return 0;
}

// 完全一样
uint16_t getJointRevision(int a)
{
	return calval[a].revision;
}

// 大致一样
uint32_t getDataForCRC(int joint_id, int a2)
{
	uint16_t temp1;
	uint16_t temp2;
	uint32_t ret = 0;

	switch (a2) {
	case 0:
		temp1 = calval[joint_id].motor_encoder_commutation_offset;
		ret = temp1 | (calval[joint_id].joint_id << 0x10);
		break;
	case 1:
		ret = calval[joint_id].motor_encoder_to_joint_factory_calibration_value;
		break;
	case 2:
		ret =  calval[joint_id].joint_encoder_factory_calibration_value;
		break;
	case 3:
		temp2 = calval[joint_id].AD_Voltage_offset_calibration;
		ret = temp2 | (calval[joint_id].joint_hardware_flags << 0x10);
		break;
	default:
		ret = calval[joint_id].checksum_part_A;  // 此处寻址有问题
		break;
	}
	return ret;
	/*
	if (a2 == 1) {
		return calval[joint_id].motor_encoder_to_joint_factory_calibration_value;
	} else	{
		if (a2 <= 1) {
			if (a2) {
				return calval[joint_id].checksum_part_A;
			} else {
				return calval[joint_id].motor_encoder_commutation_offset
								|= (calval[joint_id].joint_id << 0x10);
			}
		} else {
			if (a2 == 2) {
				return calval[joint_id].joint_encoder_factory_calibration_value;
			} else {
				if(a2 == 3) {
					return calval[joint_id].AD_Voltage_offset_calibration
									|= (calval[joint_id].joint_hardware_flags << 0x10);
				} else {
					return calval[joint_id].checksum_part_A;
				}
			}
		}
	}
	*/
}

// shit code but exactly the same
uint32_t getJointChecksum(int joint_id)
{
	uint32_t edi = 0;
	uint32_t ebx = 0;
	uint32_t temp;
	int32_t ecx;
	uint32_t edx;
	uint32_t eax;
OUT:
	temp = joint_id;
	uint32_t esi = 0;
	ecx = getDataForCRC(joint_id, edi);
IN:
	edx = ecx;
	eax = ebx;
	edx >>= 0x1F;
	eax >>= 0x1F;

	if (!(edx - eax)) {
		ebx *= 2;
		if (!(++esi - 0x20)) { // loc_80005B8
			if (++edi - 0x36)
				goto OUT;
		} else {
			ecx *= 2;
			goto IN;
		}
	} else {  // loc_8000592
		ebx = (ebx * 2) ^ 0xEDB88320;
		if (++esi - 0x20) {
			ecx *= 2;
			goto IN;
		} else {
			if ((++edi - 0x36))
				goto OUT;
		}
	}
	return ~ebx;
}

// 有问题
void parseFullPositionMessage(int joint_id)
{
	uint32_t temp1 = inbuffer[read_position++] << 0x18;
	uint32_t temp2 = inbuffer[read_position++] << 0x10;
	temp1 |= temp2;

	uint32_t temp3 = inbuffer[read_position++] << 0x8;
	temp1 |= temp3;

	uint32_t temp4 = inbuffer[read_position++];
	temp1 |= temp4;

	uint16_t temp5 = inbuffer[read_position++] << 0x8;
	uint32_t temp6 = inbuffer[read_position++];
	temp6 |= temp5;

	urEthernetComm_setLastKnownPosition(joint_id, temp1, temp6);
}

// 大致一样
void prettyPrintCalibrationData(int joint_id)
{
	uint32_t esi = 0;
	uint8_t* pcv;
	printf("[1;1H");

loop:
	pcv = (uint8_t*)&calval[joint_id].flash_write_count + esi;
	printf("\n\r %04X: ", esi);

	for(;;) {
		esi++;
		printf(" %02X ", *pcv);

		if (esi <= 0xFB) {
			pcv++;
			if (!(esi & 0xF))
				goto loop;
		} else
			break;

/*
		 if (esi <= 0xFB)
			 pcv++;
		 else
			 break;

		 if (!(esi & 0xF))
				goto loop;
*/
	}

	printf("\n\r");
	printf(" flash_write_count %04X, spare %04X \n",	calval[joint_id].flash_write_count, calval[joint_id].joint_id);
	printf(" motor_encoder_calibration %04X, joint_encoder_calibration %08X \r",
		calval[joint_id].motor_encoder_commutation_offset, calval[joint_id].joint_encoder_factory_calibration_value);
}

// 大致一样
void calibration_overwrite_motorcommand(int joint_id, struct robotcommd* probotcommd)
{
	uint8_t temp;
	long double ld;

	if (! (getting_calibration_data_from_joint[joint_id] - 1))
		probotcommd->jointcommand[joint_id] = CALIBRATION_DATA_REQUEST;  // loc_80007A5
	else {
		temp = sending_calibration_data_to_joint[joint_id];  // movzx   eax, ds:sending_calibration_data_to_joint[ebx]

		switch (temp) {
		case 1:
			probotcommd->jointcommand[joint_id] = CALIBRATION_DATA;
			sending_calibration_data_to_joint[joint_id] = 2;
			break;
		case 2:
			break;  // jz      short loc_8000784
		case 3:
			probotcommd->jointcommand[joint_id] = CALIBRATION_DATA_WRITE;// jz      short loc_8000790
			calibration_is_ok[joint_id] = 1;
			break;
		default:
			if (calibration_checksum_part_A_ok[joint_id]) {
				probotcommd->jointcommand[joint_id] = CALIBRATION_DATA_WRITE;// jz      short loc_8000790
				calibration_is_ok[joint_id] = 1;
			} else {
				if(!calibration_checksum_part_A_file_ok[joint_id]) {  // jz      short loc_80007C8
					ld = robotinterface_get_time_per_step();
					messagequeue_add_message((long)ld, (int8_t)joint_id, CALIBRATION_ANOMALY, MOTOR_TEST_FAILED);
					manufactureArtificialEmptyCalibrationData(joint_id);
					calibration_checksum_part_A_file_ok[joint_id] = 1;
				}
				probotcommd->jointcommand[joint_id] = PLEASE_REQUEST_CALIBRATION_DATA_COMMAND;
			}
		}
	}
}

/****************************************************************************************************
 * @ joint_id  关节ID
 * 功能：将calval数据结构中的数据拷贝到conf.calibration这个数据结构中
 ****************************************************************************************************/
void copy_to_configuration_calibration(int joint_id)
{
	int i,var_1C;
	int8_t bl;
   double measurement_voltage, data1, data2;
	/*
	 * for(i=0;i<8;i++)
		conf.calibration.self_test_measurement_speed[i][joint_id] = 0;
	 */
	conf.calibration.self_test_measurement_speed[0][joint_id] = 0.0;
	conf.calibration.self_test_measurement_speed[1][joint_id] = 0;   // conf+0CD8h
	conf.calibration.self_test_measurement_speed[2][joint_id] = 0;   // conf+0D08h
	conf.calibration.self_test_measurement_speed[3][joint_id] = 0;   // conf+0D38h
	conf.calibration.self_test_measurement_speed[4][joint_id] = 0;   // conf+0D68h
	conf.calibration.self_test_measurement_speed[5][joint_id] = 0;   // conf+0D98h
	conf.calibration.self_test_measurement_speed[6][joint_id] = 0;   // conf+0DC8h
	conf.calibration.self_test_measurement_speed[7][joint_id] = 0;   // conf+0DF8h
	conf.calibration.joint_revision[joint_id] = calval[joint_id].revision;

	if (calval[joint_id].data[12])
	{
		conf.calibration.self_test_completed[joint_id]=0;
	    return;
	}
	//8A0
	conf.calibration.self_test_completed[joint_id]=1;
	if(jointCompletedSelfTestWithFW1_5(joint_id)!=1)
	{
		conf.calibration.self_test_measurement_speed[0][joint_id]=1.5;
		conf.calibration.self_test_measurement_voltage[0][joint_id]=calval[joint_id].data[0];
		var_1C=0;
	}
	else//CA7
	{
		printf("***********");
		data1 = (double)calval[joint_id].data[0];
		data2 = (double)calval[joint_id].data[15];
		conf.calibration.self_test_measurement_speed[0][joint_id]=1.5;
		conf.calibration.self_test_measurement_voltage[0][joint_id] = sqrt(data1*data1+data2*data2);
		var_1C=1;
	}
	//8DE
	conf.calibration.self_test_measurement_current[0][joint_id]=calval[joint_id].data[1];//current
	bl = joint_id<=1?1:0;
	conf.calibration.self_test_measurement_temp[0][joint_id]=calval[joint_id].data[2];//temperature
	if(conf.robot_type==2 && bl)//UR10
		conf.calibration.self_test_measurement_speed[1][joint_id]=2.3;
	else//919
	   //UR5
		conf.calibration.self_test_measurement_speed[1][joint_id]=3.2;
	//931
   if(var_1C!=1)
    {
	   conf.calibration.self_test_measurement_voltage[1][joint_id]=calval[joint_id].data[3];//voltage q-axis
	   conf.calibration.self_test_measurement_speed[2][joint_id]=-1.5;
	   conf.calibration.self_test_measurement_current[1][joint_id]=calval[joint_id].data[4];//current
	   conf.calibration.self_test_measurement_temp[1][joint_id]=calval[joint_id].data[5];//temperature
	   measurement_voltage = calval[joint_id].data[6];
    }
   else//BBA
    {
		printf("***********");
	   data1 = (double)calval[joint_id].data[3];
	   data2 = (double)calval[joint_id].data[16];
	   conf.calibration.self_test_measurement_voltage[1][joint_id] = sqrt(data1*data1+data2*data2);
	   conf.calibration.self_test_measurement_speed[2][joint_id]=-1.5;
	   conf.calibration.self_test_measurement_current[1][joint_id]=calval[joint_id].data[4];
	   conf.calibration.self_test_measurement_temp[1][joint_id]=calval[joint_id].data[5];
	   measurement_voltage = sqrt(calval[joint_id].data[6]*calval[joint_id].data[6]+calval[joint_id].data[17]*calval[joint_id].data[17]);
    }
    //98B
   conf.calibration.self_test_measurement_voltage[2][joint_id]=measurement_voltage;
   conf.calibration.self_test_measurement_current[2][joint_id]=calval[joint_id].data[7];
   conf.calibration.self_test_measurement_temp[2][joint_id]=calval[joint_id].data[8];
/*
   if(!bl && conf.robot_type==2)//9C2
	   conf.calibration.self_test_measurement_speed[3][joint_id]=-3.20;//UR10
   else//B26
	   conf.calibration.self_test_measurement_speed[3][joint_id]=-2.30;//UR5
	   */
   if(bl && conf.robot_type==2)//B26
	   conf.calibration.self_test_measurement_speed[3][joint_id]=-2.30;//UR5
   else//9C2
	   conf.calibration.self_test_measurement_speed[3][joint_id]=-3.20;//UR10

   //9DA
   /*
   if(var_1C!=1)
	   conf.calibration.self_test_measurement_voltage[3][joint_id]=calval[joint_id].data[9];
   else
	   conf.calibration.self_test_measurement_voltage[3][joint_id]=sqrt(calval[joint_id].data[9]+calval[joint_id].data[18]*calval[joint_id].data[18]);
   */
   data1 = calval[joint_id].data[9];
   data2 = calval[joint_id].data[18];
   conf.calibration.self_test_measurement_voltage[3][joint_id] = var_1C==1 ? sqrt(data1*data1 + data2*data2) : data1;
   //9F8
   conf.calibration.self_test_measurement_current[3][joint_id]=calval[joint_id].data[10];
   conf.calibration.self_test_measurement_temp[3][joint_id]=calval[joint_id].data[11];

   if(conf.calibration.joint_revision[joint_id]>3 && calval[joint_id].data[22]!=-1)
    {
	   conf.calibration.self_test_measurement_speed[4][joint_id]=0.5;
	   conf.calibration.self_test_measurement_speed[5][joint_id]=-0.5;
	   conf.calibration.self_test_measurement_speed[6][joint_id]=0.05;
	   conf.calibration.self_test_measurement_speed[7][joint_id]=-0.05;
	   //剩下的初始化
      for(i=0;i<4;i++)
       {
    	  if(var_1C!=1)
    	  {
    		  conf.calibration.self_test_measurement_voltage[4+i][joint_id]=calval[joint_id].data[22+4*i];
    	  }
    	  else//B50
    	  {
    		  conf.calibration.self_test_measurement_voltage[4+i][joint_id] =
    				  sqrt(calval[joint_id].data[21+4*i]*calval[joint_id].data[21+4*i]+calval[joint_id].data[22+4*i]*calval[joint_id].data[22+4*i]);
    	  }
    	  //AE6
    	  conf.calibration.self_test_measurement_current[4+i][joint_id] = calval[joint_id].data[23+8*i];
    	  conf.calibration.self_test_measurement_temp[4+i][joint_id] = calval[joint_id].data[24+8*i];

       }
    }

}



//意思一样  xzf
struct CalibrationValues unk_calval[6];
int calibration_parse_new_calibration_data(int arg0, uint8_t arg4, uint8_t arg8)
{
	uint8_t vd = arg4 +1;
	uint8_t ve = arg4;
	uint8_t vf = arg8;
	uint32_t edi,ebx,esi;
	uint8_t ecx = inbuffer[arg4];

	if (vd > vf){
		edi = calval[arg0].flash_write_count;
		ebx = calval[arg0].flash_write_count + ecx;
		esi = unk_calval[arg0].flash_write_count;
LOOP:	if (esi < ebx || edi > ebx ){
			puts("ERROR: THE CALIBRATION DATA POINTER IS OUT OF BOUNDS");
			read_position = vf;
			if(vf > vd)
				goto LOOP;
			else
				goto LOOP1;
		}
		else{
			ebx = inbuffer[vd];
			ebx++;
			vd++;
			if (vf > vd)
				goto LOOP;
			else
				goto LOOP1;
		}
	}
LOOP1:if (vf != vd){
		printf("calibration_parse_new_calibration_data:: read_position!=end_position %i  %i",
				read_position,vf);
	}
	vd = vd - ve;
	return vd;
}

//基本一样 xzf
void write_joint_calibration_file_human_readable(int joint_id)
{
	char s[64];
	FILE* pFile;
	int i;
	double temp;
	double db8002408 = 0.000030517578125;
	double db8002400 = 3.141592653589793;

	sprintf(s,"%s/joint%i_calibration.txt",conf.path,joint_id);
	pFile = fopen(s, "w");
	if (pFile == NULL){
		perror("write_joint_calibration_file_human_readable():");
		return;
	}
	fprintf(pFile, "flash_write_count: %d\n",calval[joint_id].flash_write_count);
	fprintf(pFile,"joint_id: %d\n",calval[joint_id].joint_id);
	fprintf(pFile,"motor_encoder_commutation_offset: %d\n",calval[joint_id].motor_encoder_commutation_offset);
	fprintf(pFile,"revision: %d\n",calval[joint_id].revision);
	fprintf(pFile,"max_gear_stroke: %d\n",calval[joint_id].max_gear_stroke);
	fprintf(pFile,"motor_encoder_to_joint_factory_calibration_value: %d",calval[joint_id].motor_encoder_to_joint_factory_calibration_value);
	fprintf(pFile,"joint_encoder_factory_calibration_value: %d",calval[joint_id].joint_encoder_factory_calibration_value);
	fprintf(pFile, "AD_Voltage_offset_calibration: %d\n",calval[joint_id].AD_Voltage_offset_calibration);
	fprintf(pFile,"joint_hardware_flags: %02X\n",calval[joint_id].joint_hardware_flags);
	fprintf(pFile,"checksum_part_A: %d\n",calval[joint_id].checksum_part_A);
	fprintf(pFile,"AD_HallA_offset_calibration: %d\n",calval[joint_id].AD_HallA_offset_calibration);
	fprintf(pFile,"AD_HallB_offset_calibration: %d\n",calval[joint_id].AD_HallB_offset_calibration);
	fprintf(pFile,"checksum_part_B: %d\n",calval[joint_id].checksum_part_B);
	fprintf(pFile,"joint_revolution_counter: %d\n",calval[joint_id].joint_revolution_counter);
	fprintf(pFile,"last_seen_joint_position: %d\n",calval[joint_id].last_seen_joint_position);
	fprintf(pFile,"checksum_part_C: %d\n",calval[joint_id].checksum_part_C);

	fwrite("data: [",1,7,pFile);
	for (i=0; i!= 99; i++){
		fprintf(pFile,"%d, ",calval[joint_id].data[i]);
	}
	fprintf(pFile, "%d]\n",calval[joint_id].data[99]);
	fprintf(pFile,"checksum_part_D: %d\n",calval[joint_id].checksum_part_D);
	fprintf(pFile,"joint_checksum: 0x%x\n",getJointChecksum(joint_id));
	if (calval[joint_id].revision <= 8){
		fprintf(pFile,"eccentricy_tau: %.5f \n",calval[joint_id].data[32]*db8002408*db8002400);
		fprintf(pFile,"eccentricy_sigma: %.5f \n",calval[joint_id].data[33]*db8002408*db8002400);
		temp = calval[joint_id].data[35]<<16 + calval[joint_id].data[36];
		fprintf(pFile, "eccentricy_mean: %.5f\n",temp/calval[joint_id].data[37]*db8002400*db8002408 );
		fprintf(pFile,"eccentricy_max:  %.5f\n",calval[joint_id].data[37]*db8002400*db8002408);
	}
	fclose(pFile);
}

//完全一样 xzf
void write_joint_calibration_file(int joint_id)
{
	char s[64];
	FILE* pFile;
	double interfacetime;

	sprintf(s, "%s/joint%i_calibration.dat", conf.path, joint_id);
	pFile = fopen(s, "w");
	if(pFile == NULL){
		perror("write_joint_calibration_file(): ");
		interfacetime = robotinterface_get_time();
		messagequeue_add_message(interfacetime, joint_id, 0xB7, 0x57);
		return;
	}else{
	fwrite(&calval[joint_id].flash_write_count, 0xFC, 1, pFile);
	fclose(pFile);
	}
}

// 大致一样 xzf
int read_joint_calibration_file(char* buff, int joint_id)
{
	char s[64];  //filename
	double interfacetime;
	FILE* pFile;
	struct stat stat_buf;
	size_t result;
	int ret;

	sprintf(s,"%s/joint%i_calibration.dat",conf.path,joint_id);

	if(__xstat(3,s,&stat_buf)){
		perror("read_joint_calibration_file(): ");
		return 0;
	}
	if(stat_buf.st_size == 0x30 || stat_buf.st_size == 0xFC){
		pFile = fopen(s,"r");
		if (pFile == NULL){
			interfacetime = robotinterface_get_time();
			messagequeue_add_message(interfacetime, joint_id,0xB7, 0x56);
			perror("read_joint_calibration_file(): ");
			ret = 0;
		}else{
			if(stat_buf.st_size ==0x30){
				result = fread(buff,1,0xFC,pFile);
			}else{
				result = fread(buff,1,0x30,pFile);
			}
			fclose(pFile);
			ret = (result == 0xFC) || (result == 0x30);
		}
	}else{
		interfacetime = robotinterface_get_time();
		messagequeue_add_message(interfacetime, joint_id,0xB7, 0x55);
		ret = 0;
	}
	return ret;
}

void parseCalibrationData(int joint_id)
{
	 uint8_t var_125;
    int read_offset_edx, esi;
    uint32_t parse_number_120;
    int jointmode, errorcode;
    uint8_t errorArgument;
    double time;
    uint8_t *pbuff_11c, *pbuff_start, pCalibuff[252], ptemp124[12];
    //uint16_t checksum_part_A;
     //第一个字节表示要解析的节数
    parse_number_120 = var_125 = inbuffer[read_position++];
     //第二个字节表示在CalibrationValues数据结构中解析到第几个字节
    read_offset_edx = inbuffer[read_position++];

    pbuff_11c = (uint8_t *)&calval[joint_id];
    pbuff_11c += read_offset_edx;
    pbuff_start = pbuff_11c;

    if(parse_number_120 > 0)
     {
    	*pbuff_11c++ = inbuffer[read_position++];
    	calibration_data_read_offset[joint_id]++;

       if(pbuff_11c > calval[joint_id].data)
        {
          //超过int16_t data[100]
          //1934
    	   time = robotinterface_get_time();
    	   jointmode = JOINT_INITIALISATION_MODE;
    	   errorcode = ENCODER_JOINT_INTERRUPT_WITH_TWO_STEP_CHANGE_ERROR;//22
    	   errorArgument = joint_id;
        }
       else//156E
        {
           //未超过int16_t data[100]
         while(calval[joint_id].data <= pbuff_11c)
           {
				   if(pbuff_11c == calval[joint_id].data)
					{
					   getting_calibration_data_from_joint[joint_id] =0;
					    //检查ID
					   if(joint_id != calval[joint_id].joint_id && calval[joint_id].joint_id != 0xFF)
						{
							time = robotinterface_get_time();
							jointmode = joint_id;
							errorcode = CALIBRATION_ANOMALY;//0xB7
							errorArgument = 0x1D;
							messagequeue_add_message((time_t)time,jointmode,errorcode,errorArgument);

							calval[joint_id].joint_id = joint_id;
							calval[joint_id].checksum_part_A = calculateChecksumForPartA(joint_id);

							write_joint_calibration_file(joint_id);

							calval[joint_id].checksum_part_A++;
						}
						//1624
					   //检查part_A
					  if( calval[joint_id].checksum_part_A !=calculateChecksumForPartA(joint_id))
					   {
						  calibration_checksum_part_A_ok[joint_id]=0;
						  messagequeue_add_message((time_t)robotinterface_get_time(joint_id),joint_id,CALIBRATION_CHECKSUM_A_FAILED,1);

						  if(read_joint_calibration_file(pCalibuff,joint_id))
						  {
							   memcpy(&calval[joint_id],pCalibuff,48);
							   //check again
							  if(calculateChecksumForPartA(joint_id) != calval[joint_id].checksum_part_A)
							  {
								  messagequeue_add_message((time_t)robotinterface_get_time(joint_id),joint_id,CALIBRATION_CHECKSUM_A_FAILED,2);//最后一个形参是比较次数
								  calibration_checksum_part_A_ok[joint_id]=0;
							  }
							  else//1972
								  calibration_checksum_part_A_ok[joint_id]=1;
						  }
						  else//18DA
						  {
							//读取失败
							  messagequeue_add_message((time_t)robotinterface_get_time_per_step(),JOINT_INITIALISATION_MODE,NO_CALIBRATION_FILE_FOUND,joint_id);
							  calibration_checksum_part_A_file_ok[joint_id]=0;
						  }
					   }
					  else//1750
						  calibration_checksum_part_A_ok[joint_id]=1;

					//175A
					   //检查Part C部份
					if(calculateChecksumForPartC(joint_id)!= calval[joint_id].checksum_part_C)
						   messagequeue_add_message((time_t)robotinterface_get_time_per_step(),joint_id,CALIBRATION_CHECKSUM_C_FAILED,1);

					//17C7
					//检查版本号
					if(calval[joint_id].revision)
					{
						if(calculateChecksumForPartD(joint_id)!=calval[joint_id].checksum_part_D)
							messagequeue_add_message((time_t)robotinterface_get_time(),joint_id, CALIBRATION_CHECKSUM_A_FAILED,3);
						else//1958
						{
							copy_to_configuration_calibration(joint_id);
							calibration_checksum_part_A_ok[joint_id]=1;
						}
					 }
					 else//1855
						  calibration_set_revision_zero(joint_id);

					  //1838
				   if(calibration_checksum_part_A_ok[joint_id])
						 write_joint_calibration_file(joint_id);//OK

				   }//if(pbuff== calval[joint_id].data)
				   //1527
				  //未解析完
				 if(pbuff_11c != pbuff_start + parse_number_120)
				 {
					  *pbuff_11c++ =inbuffer[read_position++];
					  calibration_data_read_offset[joint_id]++;

				 }
				else
					break;
          }//while
          //1868
        if(parse_number_120 <= 7 && pbuff_11c != calval[joint_id].data)
          {
    	   time = robotinterface_get_time();
    	   jointmode = joint_id;
    	   errorcode = CALIBRATION_ANOMALY;
    	   errorArgument = 0xA;

          }
        else
        	 return;

      }//else 156E

    }//if(parse_number_120 > 0)
    else//1868
     {
    	 if(parse_number_120 <= 7 && pbuff_11c != calval[joint_id].data)
    	 {

    	     time = robotinterface_get_time_per_step();
    	     jointmode = joint_id;
    	     errorcode = CALIBRATION_ANOMALY;
    	     errorArgument = 0xA;

    	  }
    	 else
    		 return;
     }
    //18A3
    messagequeue_add_message((time_t)time, jointmode, errorcode, errorArgument);
}


